#include "IMU.h"
#include "Define.h"
#include "i2c.h"

#define I2C_TIMEOUT 10

#define	SMPLRT_DIV		0x19	//陀螺仪采样率分频寄存器，典型值：0x07(125Hz)。采样计算式： 采样频率 = 陀螺仪输出频率 / （SMPLRT_DIV + 1）
#define	CONFIGL			0x1A	//配置寄存器，典型值：0x06(5Hz)。
#define	GYRO_CONFIG		0x1B	//陀螺仪配置寄存器，典型值：0x18(使用陀螺仪的满量程 2000deg/s)
#define	ACCEL_CONFIG	0x1C	//加速度传感器配置寄存器，典型值：0x01(不自检，5Hz，使用加速度计+-2g的量程范围)
#define	PWR_MGMT_1		0x6B	//电源管理寄存器，典型值：0x00(正常启用)

/*0x3b~0x40 是加速度传感器数据输出寄存器*/
#define	ACCEL_ADDRESS	0x3B 
#define	ACCEL_XOUT_H	0x3B
#define	ACCEL_XOUT_L	0x3C
#define	ACCEL_YOUT_H	0x3D
#define	ACCEL_YOUT_L	0x3E
#define	ACCEL_ZOUT_H	0x3F
#define	ACCEL_ZOUT_L	0x40
/************************************/

/*0x41~0x42 是温度传感器数据输出寄存器*/
#define	TEMP_OUT_H		0x41
#define	TEMP_OUT_L		0x42
/************************************/

/*0x43~0x48 是陀螺仪数据的输出寄存器*/
#define GYRO_ADDRESS  0x43
#define	GYRO_XOUT_H		0x43
#define	GYRO_XOUT_L		0x44	
#define	GYRO_YOUT_H		0x45
#define	GYRO_YOUT_L		0x46
#define	GYRO_ZOUT_H		0x47
#define	GYRO_ZOUT_L		0x48
/************************************/

#define MPU6050_CLOCK_INTERNAL          0x00
#define MPU6050_CLOCK_PLL_XGYRO         0x01
#define MPU6050_CLOCK_PLL_YGYRO         0x02
#define MPU6050_CLOCK_PLL_ZGYRO         0x03
#define MPU6050_CLOCK_PLL_EXT32K        0x04
#define MPU6050_CLOCK_PLL_EXT19M        0x05
#define MPU6050_CLOCK_KEEP_RESET        0x07

#define MPU6050_DLPF_BW_256         0x00
#define MPU6050_DLPF_BW_188         0x01
#define MPU6050_DLPF_BW_98          0x02
#define MPU6050_DLPF_BW_42          0x03
#define MPU6050_DLPF_BW_20          0x04
#define MPU6050_DLPF_BW_10          0x05
#define MPU6050_DLPF_BW_5           0x06

#define MPU6050_GYRO_FS_250         0x00
#define MPU6050_GYRO_FS_500         0x01
#define MPU6050_GYRO_FS_1000        0x02
#define MPU6050_GYRO_FS_2000        0x03

#define MPU6050_ACCEL_FS_2          0x00
#define MPU6050_ACCEL_FS_4          0x01
#define MPU6050_ACCEL_FS_8          0x02
#define MPU6050_ACCEL_FS_16         0x03

#define MPU6050_RA_WHO_AM_I         0x75
#define MPU6050_ADDRESS 0xD0 //本来是0x68，但是是硬件IIC所以就0XD0

float Acc_Scale = 0;
float Gyr_Scale = 0;

/****************************************************************************************
*@brief  
*@brief   
*@param[in]
*****************************************************************************************/
void MPU6050_DeviceReset(uint8_t Reset)
{
	uint8_t tmp;
	HAL_I2C_Mem_Read(&hi2c1, MPU6050_ADDRESS, PWR_MGMT_1, 1, &tmp, 1, I2C_TIMEOUT);
	tmp &= ~(1<<7);
	tmp |= ((Reset & 0x1) << 7);
	HAL_I2C_Mem_Write(&hi2c1, MPU6050_ADDRESS, PWR_MGMT_1, 1, &tmp, 1, I2C_TIMEOUT);
}

/****************************************************************************************
*@brief  
*@brief   
*@param[in]
*****************************************************************************************/
void MPU6050_SetSleepEnabled(uint8_t Enable)
{
	uint8_t tmp;
	HAL_I2C_Mem_Read(&hi2c1, MPU6050_ADDRESS, PWR_MGMT_1, 1, &tmp, 1, I2C_TIMEOUT);
	tmp &= ~(1<<6);
	tmp |= ((Enable & 0x1) << 6);
	HAL_I2C_Mem_Write(&hi2c1, MPU6050_ADDRESS, PWR_MGMT_1, 1, &tmp, 1, I2C_TIMEOUT);
}


/****************************************************************************************
*@brief  
*@brief   
*@param[in]
*****************************************************************************************/
void MPU6050_SetClockSource(uint8_t Source)
{
	uint8_t tmp;
	HAL_I2C_Mem_Read(&hi2c1, MPU6050_ADDRESS, PWR_MGMT_1, 1, &tmp, 1, I2C_TIMEOUT);
	tmp &= 0xF8;
	tmp |= (Source & 0x7);
	HAL_I2C_Mem_Write(&hi2c1, MPU6050_ADDRESS, PWR_MGMT_1, 1, &tmp, 1, I2C_TIMEOUT);
}


/****************************************************************************************
*@brief  
*@brief   
*@param[in]
*****************************************************************************************/
void MPU6050_SetDlpf(uint8_t Value)
{
	uint8_t tmp;
	HAL_I2C_Mem_Read(&hi2c1, MPU6050_ADDRESS, CONFIGL, 1, &tmp, 1, I2C_TIMEOUT);
	tmp &= 0xF8;
	tmp |= (Value & 0x7);
	HAL_I2C_Mem_Write(&hi2c1, MPU6050_ADDRESS, CONFIGL, 1, &tmp, 1, I2C_TIMEOUT);
}


/****************************************************************************************
*@brief  
*@brief   
*@param[in]
*****************************************************************************************/
void MPU6050_SetFullScaleGyroRange(uint8_t Range)
{
	uint8_t tmp;
	HAL_I2C_Mem_Read(&hi2c1, MPU6050_ADDRESS, GYRO_CONFIG, 1, &tmp, 1, I2C_TIMEOUT);
	tmp &= 0xE7;
	tmp |= ((Range & 0x7) << 3);
	HAL_I2C_Mem_Write(&hi2c1, MPU6050_ADDRESS, GYRO_CONFIG, 1, &tmp, 1, I2C_TIMEOUT);
	switch(Range)
	{
		case MPU6050_GYRO_FS_250:
			Gyr_Scale = 0.007633;
			break;
		case MPU6050_GYRO_FS_500:
			Gyr_Scale = 0.015267;
			break;
		case MPU6050_GYRO_FS_1000:
			Gyr_Scale = 0.030487;
			break;
		case MPU6050_GYRO_FS_2000:
			Gyr_Scale = 0.060975;
			break;
		default:
			break;
	}
}

void MPU6050_SetFullScaleAccelRange(uint8_t Range)
{
	uint8_t tmp;
	HAL_I2C_Mem_Read(&hi2c1, MPU6050_ADDRESS, ACCEL_CONFIG, 1, &tmp, 1, I2C_TIMEOUT);
	tmp &= 0xE7;
	tmp |= ((Range & 0x7) << 3);
	HAL_I2C_Mem_Write(&hi2c1, MPU6050_ADDRESS, ACCEL_CONFIG, 1, &tmp, 1, I2C_TIMEOUT);
	switch(Range)
	{
		case MPU6050_ACCEL_FS_2:
			Acc_Scale = 0.000061;
			break;
		case MPU6050_ACCEL_FS_4:
			Acc_Scale = 0.000122;
			break;
		case MPU6050_ACCEL_FS_8:
			Acc_Scale = 0.000244;
			break;
		case MPU6050_ACCEL_FS_16:
			Acc_Scale = 0.0004882;
			break;
		default:
			break;
	}
}

/****************************************************************************************
*@brief  
*@brief   
*@param[in]
*****************************************************************************************/
void MPU6050_GetGyroscopeRAW(int16_t *x, int16_t *y, int16_t *z)
{
	uint8_t tmp[6];
	HAL_I2C_Mem_Read(&hi2c1, MPU6050_ADDRESS, GYRO_ADDRESS, 1, tmp, 6, I2C_TIMEOUT);

	*x = (((int16_t)tmp[0]) << 8) | tmp[1];
	*y = (((int16_t)tmp[2]) << 8) | tmp[3];
	*z = (((int16_t)tmp[4]) << 8) | tmp[5];
}

/****************************************************************************************
*@brief  
*@brief   
*@param[in]
*****************************************************************************************/
void MPU6050_GetAccelerometerRAW(int16_t *x, int16_t *y, int16_t *z)
{
	uint8_t tmp[6];
	HAL_I2C_Mem_Read(&hi2c1, MPU6050_ADDRESS, ACCEL_ADDRESS, 1, tmp, 6, I2C_TIMEOUT);

	*x = (((int16_t)tmp[0]) << 8) | tmp[1];
	*y = (((int16_t)tmp[2]) << 8) | tmp[3];
	*z = (((int16_t)tmp[4]) << 8) | tmp[5];
}


/****************************************************************************************
*@brief  
*@brief   
*@param[in]
*****************************************************************************************/
void MPU_GetInformation(float* ax, float* ay, float* az, float* gx, float* gy, float* gz)
{
	int16_t tmp_x, tmp_y, tmp_z;

	MPU6050_GetAccelerometerRAW(&tmp_x, &tmp_y, &tmp_z);
	*ax = (float)tmp_x * Acc_Scale;
	*ay = (float)tmp_y * Acc_Scale;
	*az = (float)tmp_z * Acc_Scale;

	MPU6050_GetGyroscopeRAW(&tmp_x, &tmp_y, &tmp_z);
	*gx = (float)tmp_x * Gyr_Scale;
	*gy = (float)tmp_y * Gyr_Scale;
	*gz = (float)tmp_z * Gyr_Scale;
}


/****************************************************************************************
*@brief  
*@brief   
*@param[in]
*****************************************************************************************/
int16_t MPU6050_GetTemperatureRAW(void)
{
	uint8_t tmp[2];
	HAL_I2C_Mem_Read(&hi2c1, MPU6050_ADDRESS, TEMP_OUT_H, 1, tmp, 2, I2C_TIMEOUT);
	return (((int16_t)tmp[0]) << 8) | tmp[1];
}

/****************************************************************************************
*@brief  
*@brief   
*@param[in]
*****************************************************************************************/
float MPU6050_GetTemperatureCelsius(void)
{
	int16_t temp;

	temp = MPU6050_GetTemperatureRAW();

	return (float)temp / 340 + 36.53;
}

/****************************************************************************************
*@brief  
*@brief   
*@param[in]
*****************************************************************************************/
uint8_t MPU6050_GetDeviceID(void)
{
	uint8_t tmp;
	HAL_I2C_Mem_Read(&hi2c1, MPU6050_ADDRESS, MPU6050_RA_WHO_AM_I, 1, &tmp, 1, I2C_TIMEOUT);
	return tmp<<1;
}

/****************************************************************************************
*@brief  
*@brief   
*@param[in]
*****************************************************************************************/
void MPU6050_Init(void)
{
	MPU6050_DeviceReset(1);
    MPU6050_SetSleepEnabled(0);
    MPU6050_SetClockSource(MPU6050_CLOCK_INTERNAL);
    MPU6050_SetDlpf(MPU6050_DLPF_BW_42);
    MPU6050_SetFullScaleGyroRange(MPU6050_GYRO_FS_2000);
    MPU6050_SetFullScaleAccelRange(MPU6050_ACCEL_FS_4);
}